#ifndef RELOCATION_SLAM_CERESBUNDLE_H
#define RELOCATION_SLAM_CERESBUNDLE_H

#include "mappoints_costFunction.h"

using namespace ceres;
using namespace std;

void SolveProblem(vector<Relocation_Slam::Frame*> f,vector<Relocation_Slam::MapPoint>& mappoints){

    ceres::Problem problem;

    for(int i=0;i<f.size();i++){
        Eigen::Isometry3d cameraPose = f[i]->mPose.inverse();  ///<转到相机系　Ｔcw
        for(int j=0;j<f[i]->mvKeyPoints.size();j++){
            if(f[i]->mbKpTriangle[j] == true && f[i]->mnMappointId[j]!=-1){
                double x = f[i]->mvKeyPoints[j].pt.x;
                double y = f[i]->mvKeyPoints[j].pt.y;

                CostFunction* cost_function;

                cost_function = MappointsCostFunction::Create(x,y,cameraPose);

                LossFunction* loss_function = new HuberLoss(1.0);

                problem.AddResidualBlock(cost_function, loss_function, mappoints[f[i]->mnMappointId[j]].mPose);
            }
        }
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
//    std::cout << summary.FullReport() << "\n";
}
#endif
